/*	IMU.c & .h
 *	
 *	HUV Robotics IMU interface module.
 *	The raw gyro & accelerometer data from the HUV Robotics IMU is combined to
 *	calculate Pitch & Roll.  Outputs are calibrated in radians.
 *	The module does not provide any yaw feedback (even though the IMU has a a yaw
 *	gyro) since there is no compass to cancel out yaw gyro drift.
 *	The module expects IMUInitialise() to be called at the start of the program then
 *	IMUHeartBeat() to be called 128 times per second.  IMU output can then be read
 *	via IMURead().
 *	IMURadiansToDegrees() can be used if output in degrees is required.
 *
 *	Author:  Phil Hall
 *	Date:    May 2008
 *
*/

/*
	Complementary filter added 06/07/09
	Alexandre Vicente
*/
#ifndef IMU_H
#define IMU_H

#define DEFAULT_IMU_ID			120
#define IMU_HB_TICKS			6	//	Heart beat processed 128 / IMU_HB_TICKS times per second


#define X_zero					329
#define	Y_zero					330
#define	Z_zero					315
#define	Pitch_zero				363
#define	Roll_zero				292


typedef struct
{							//	Calibrated in radians
	double		Pitch,		//	-pi/2: laying face down, +pi/2: laying flat on back, 0: upright
				Roll;		//	-pi/2: laying on right arm, +pi/2: laying on left arm, 0: upright
}	IMU_DATA;

typedef struct
{
	unsigned short		AccelSide,
						AccelFore,
						AccelVert,
						GyroPitch,
						GyroRoll,
						GyroYaw,
						Dig1,
						Dig2,
						Dig3,
						Dig4;
}	IMU_RAW;

typedef struct
{
	long				AccelSide,
						AccelFore,
						AccelVert,
						GyroPitch,
						GyroRoll,
						GyroYaw;
}	IMU_ZERO_READS;

typedef struct //Complementary Filter component
{
	double				Pitch,
						Pitch_Raw,
						Pitch_Offset,
						Roll,
						Roll_Raw,
						Roll_Offset,
						Vector_Mag,
						Vector_Angle;
}	AXIS;



int IMUInitialise(unsigned char IMU_ID);
	//	Set IMU_ID to zero to use DEFAULT_IMU_ID

void IMUHeartBeat(void);

int IMURead(IMU_DATA *IMUData);

double IMUArcSin(double V);
	//	Ensures -1 <= V <= 1 before passing it to asin()

int IMURadiansToDegrees(double Radians);

void IMUGetRaw(unsigned char IMUID, IMU_RAW *Raw);

void IMUGetFiltered(unsigned char IMUID, AXIS *Axis,double Offset_Pitch,double Offset_Roll); //The Complementary Filter

double LowPass_Pitch(double In);//Complementary Filter Component

double LowPass_Roll(double In);//Complementary Filter Component

void IMUTestMode (int Mode);
	//	Mode 0:	Test mode disabled.
	//	Mode 1:	Waits for the IMU to intialise then "Randomly" moves a servo (default 6) constantly outputing the
	//			servo position, IMU pitch & servo speed. This can be exported to Excel to compare in a graph.
	//			The test servo should be mounted with its mid point (note marks on servo case) upright.  The test
	//			is then limited so that measurements are only conducted in the top 180 degrees of travel.

#endif

/*

Joerg's code to convert accelerometer readings into a G vector.

			_IMUData.Pitch = Raw.AccelFore - IMUZero.AccelFore;
			// measured: 70 units in Accel when pointing to the ground, calculated 62 units is pointing to the ground (9.81)
		
			double g = sqrt( (Raw.AccelFore - IMUZero.AccelFore)*(Raw.AccelFore - IMUZero.AccelFore) + 
							  (Raw.AccelSide - IMUZero.AccelSide)*(Raw.AccelSide - IMUZero.AccelSide) + 
							  (Raw.AccelVert - IMUZero.AccelVert)*(Raw.AccelVert - IMUZero.AccelVert) 
							);
				
			double g = 62; // Assume no acceleration by movement
			double AccelAngle = (double)(IMUArcSin((double)((double)Raw.AccelFore - (double)IMUZero.AccelFore)/g));		// angle
			_IMUData.Pitch = (int)(AccelAngle * RAD_TO_IMU_GYRO);

#define IMU_HISTORY_ENTRIES		5
#define RAD_TO_IMU_GYRO			((128 / IMU_HB_TICKS) * -23.48)
// 23.48 converts from radians IMU gyro units



*/